﻿using CWCS.Common;
using CWCS.Core.Library.PLCManager;
using CWCS.Entity;
using CWCS.Entity.Robot;
using CWCS.Entity.SRM;
using HslCommunication;
using HslCommunication.Profinet.Siemens;
using Nti56.Modbus.Communication.Models;
using Nti56.Modbus.Communication.Services;
using System;
using System.Collections.Generic;
using System.Configuration;
using System.Linq;
using System.Net;
using System.Text;
using System.Threading;

namespace CWCS.Core.Library.ModbusManager
{
    public class RobotModus : PLCHelper
    {
        string local_ip;
        public static Dictionary<string, string> robotStatus = new Dictionary<string, string>();//机械手状态列表
        public List<RobotInfo> RobotsInfo = new List<RobotInfo>();
        private Dictionary<string, string> dicIP = new Dictionary<string, string>();
        public Dictionary<string, bool> PLCConnectStatus = new Dictionary<string, bool>();

        ModbusReadWriteService readService = new ModbusReadWriteService();
        ModbusReadWriteService writeService = new ModbusReadWriteService();

        public static Dictionary<string, ModbusTcpClient> modbusScanClients = new Dictionary<string, ModbusTcpClient>();
        public static Dictionary<string, ModbusTcpClient> modbusReadClients = new Dictionary<string, ModbusTcpClient>();
        public static Dictionary<string, ModbusTcpClient> modbusWriteClients = new Dictionary<string, ModbusTcpClient>();

        private delegate void ReadOPCInfos(RobotInfo rbinfo);
        private event ReadOPCInfos ReadRobotOPCInfo;

        private delegate void ReConnetPLC(string ip, int retryCount);
        private event ReConnetPLC RobotReConnectPLC;
        public RobotModus()
        {
            this.ReadRobotOPCInfo += new ReadOPCInfos(ReadRobotInfo);
            this.RobotReConnectPLC += new ReConnetPLC(ReConnect);
        }
        /// <summary>
        /// 初始化机械手
        /// </summary>
        /// <param name="lst"></param>
        /// <param name="error"></param>
        /// 
        public void InitData(List<RobotInfo> lst, ref string error)
        {
            try
            {
                foreach (var item in lst as List<RobotInfo>)
                {
                    local_ip = item.key_ip;
                    if (!ModbusList.ContainsKey(local_ip))
                    {
                        ModbusConnectionInfo connectionInfo = new ModbusConnectionInfo()
                        {
                            Ip = IPAddress.Parse(local_ip),
                            Port = 502,
                            Station = 1
                        };
                        ModbusList.Add(local_ip, connectionInfo);
                        PLCConnectStatus.Add(local_ip, false);

                        var Readconnect = ModbusConnection.Connection(connectionInfo, "R" + local_ip);
                        var Writeconnect = ModbusConnection.Connection(connectionInfo, "W" + local_ip);
                        var Scanconnect = ModbusConnection.Connection(connectionInfo, "Scan" + local_ip);
                        modbusReadClients[local_ip] = Readconnect.Value;
                        modbusWriteClients[local_ip] = Writeconnect.Value;
                        modbusScanClients[local_ip] = Scanconnect.Value;

                        if (Readconnect.Value.Client.Connected && Writeconnect.Value.Client.Connected)
                        {
                            if (!PLCConnectStatus.ContainsKey(item.key_ip))
                            {
                                PLCConnectStatus.Add(item.key_ip, true);
                            }
                        }
                        else
                        {
                            if (!PLCConnectStatus.ContainsKey(item.key_ip))
                            {
                                PLCConnectStatus.Add(item.key_ip, false);
                            }
                        }
                    }
                    if (!dicIP.ContainsKey(local_ip + ":" + item.robotnum))
                    {
                        dicIP.Add(local_ip + ":" + item.robotnum, item.robotnum);
                    }
                    RobotInit(item);
                }
            }
            catch (Exception ex)
            {
                error = ex.Message;
                ShowMsg("Init", "机械手初始化异常：" + ex.Message);
            }
        }
        private void RobotInit(RobotInfo rb)
        {
            if (!RobotsInfo.Contains(rb))
                RobotsInfo.Add(rb);
        }
        public void Run(object i_Index)
        {
            Thread threadCom = new Thread(new ParameterizedThreadStart(TimeScan));
            threadCom.IsBackground = true;
            threadCom.Start(i_Index);
        }
        private void TimeScan(object belong)
        {
            while (true)
            {
                try
                {
                    var lst = RobotsInfo.Where(it => it.belong == belong.ToString()).ToList();
                    if (lst.Count > 0)
                    {
                        foreach (RobotInfo rb in lst)
                        {
                            ReadRobotInfo(rb);
                        }
                    }
                }
                catch (Exception ex)
                {
                }
                Thread.Sleep(500);
            }
        }
        private void ReadRobotInfo(RobotInfo rb)
        {
            try
            {
                if (!robotStatus.ContainsKey(rb.name))
                {
                    robotStatus.Add(rb.name, "3");
                }
                int A = rb.length;
                if (ReadRobotOPCInfo != null)
                {
                    Byte[] rbBuffer = new byte[rb.length * 2];
                    if (modbusReadClients[rb.key_ip].Client.Connected && modbusWriteClients[rb.key_ip].Client.Connected && modbusScanClients[rb.key_ip].Client.Connected)
                    {
                        var result = readService.ReadByte(rb.start, (ushort)(rb.length * 2), modbusReadClients[rb.key_ip], "R" + rb.belong);
                        if (result.IsSucceed)
                        {
                            result.Value.CopyTo(rbBuffer, 0);
                            rb.m_ConnectStatus = true;

                            if (rb.stationtype == 1 || rb.stationtype == 5)//扫码校验
                            {
                                BindCheckRoboInfo(rb.robotnum, rbBuffer);
                            }
                            if (rb.stationtype == 2 || rb.stationtype == 4) //组盘
                            {
                                BindPutRoboInfo(rb.robotnum, rbBuffer);
                            }
                            if (rb.stationtype == 3)//拆盘
                            {
                                BindPickRoboInfo(rb.robotnum, rbBuffer);
                            }
                            if (rb.stationtype == 6)//码垛
                            {
                                StackingRoboInfo(rb.robotnum, rbBuffer);
                            }
                            #region 心跳值写入
                            if ((rb.stationtype == 2 || rb.stationtype == 3 || rb.stationtype == 4) && rb.heart == 1)
                            {
                                Write(rb.robotnum, "2", 10, 2);
                            }
                            #endregion
                        }
                        else
                        {
                            rb.m_ConnectStatus = false;
                            if (robotStatus.ContainsKey(rb.name))
                            {
                                robotStatus[rb.name] = "3";
                            }
                            ReConnect(rb.key_ip, 1);
                        }
                    }
                    else
                    {
                        rb.m_ConnectStatus = false;
                        if (robotStatus.ContainsKey(rb.name))
                        {
                            robotStatus[rb.name] = "3";
                        }
                        ReConnect(rb.key_ip, 1);
                    }
                    ShowUI(rb);
                }
            }
            catch (Exception ex) { ShowMsg("Init", "机械手信息读取异常：" + ex.Message); }
        }
        public override string[] ReadPLCInfo(string EquipNum, int index = 0)
        {
            throw new NotImplementedException();
        }
        /// <summary>
        /// 清理电池信息或者托盘条码尾巴
        /// </summary>
        /// <param name="barcode"></param>
        private string clearbarcodeInfo(string barcode)
        {
            char[] c = new char[5];
            c[0] = '\0';
            c[1] = '\r';
            c[2] = '\n';
            c[3] = ' ';
            c[4] = ' ';
            barcode = barcode.Replace(c[0], c[4]).Replace("\r", "").Replace("\n", "").Replace(" ", "").Replace("", "").Trim(' ');
            return barcode;
        }

        private string clearbarcodeFormat(string barcode1)
        {
            string[] str = new string[11];
            char[] c = new char[7];
            str[0] = "?m?";
            str[1] = "?";
            str[2] = "@";
            str[3] = "?n?";
            str[4] = "(";
            str[5] = "n?";
            str[6] = "?n";
            str[7] = "m?";
            str[8] = "?m";
            str[9] = "*";
            str[9] = "<";
            c[0] = '\u0003';
            c[1] = '\t';
            c[2] = '\u0013';
            c[3] = '\u0002';
            c[4] = '\u0018';
            c[5] = '\u001c';
            c[6] = '\u000e';
            string barcode = barcode1.Replace(str[0], "").Replace(str[1], "").Replace(str[2], "").Replace(str[3], "").Replace(str[4], "").Replace(str[5], "").Replace(str[6], "").Replace(str[7], "").Replace(str[8], "")
                .Replace(str[9], "").Replace(c[0], ' ').Replace(c[1], ' ').Replace(c[2], ' ').Replace(c[3], ' ').Replace(c[4], ' ').Replace(c[5], ' ').Replace(c[6], ' ').Replace(" ", "");
            return barcode;
        }




        /// <summary>
        /// 电芯信息高低位转换
        /// </summary>
        /// <param name="barcode"></param>
        private byte[] highlowbatteryInfo(byte[] batteryInfo, int batteryLength)
        {
            // byte[] batteryInfosTmp = new byte[batteryLength];
            //高低位转化
            for (int i = 0; i < batteryLength - 1; i++)
            {
                if (i % 2 == 0)
                {
                    byte BatteryCodeTmp = new byte();
                    BatteryCodeTmp = batteryInfo[i];
                    batteryInfo[i] = batteryInfo[i + 1];
                    batteryInfo[i + 1] = BatteryCodeTmp;
                }
            }
            return batteryInfo;
        }

        private void BindCheckRoboInfo(string robotnum, byte[] byt)//校验请求
        {
            RobotInfo rb = RobotsInfo.Find(
                delegate (RobotInfo ss)
                {
                    return ss.robotnum == robotnum;
                }
            );
            if (robotnum == "6002")
            {

            }
            if (rb != null)
            {
                int cellLength = 30;
                byte[] BatteryCode1Tmp = new byte[cellLength];
                byte[] BatteryCode2Tmp = new byte[cellLength];
                byte[] BatteryCode3Tmp = new byte[cellLength];
                byte[] BatteryCode4Tmp = new byte[cellLength];

                rb.wcstaskno = (byt[rb.beginvale + 0] * 256 + byt[rb.beginvale + 1]).ToString();  //WCS任务号
                rb.rbtaskno = byt[rb.beginvale + 2] * 256 + byt[rb.beginvale + 3];//机器人任务号
                rb.rbstate = byt[rb.beginvale + 4] * 256 + byt[rb.beginvale + 5];//机器人运行状态 
                rb.workstation = byt[rb.beginvale + 6] * 256 + byt[rb.beginvale + 7];//当前(装)拆盘数量
                if (rb.stationtype == 1)
                {
                    rb.inplacebarcodereading = byt[rb.beginvale + 8] * 256 + byt[rb.beginvale + 9];//机器人条码到位               
                }
                else if (rb.stationtype == 5)
                {
                    rb.checkbarcodereading = byt[rb.beginvale + 8] * 256 + byt[rb.beginvale + 9];//机器人校验条码交互信号   
                }
                rb.barcode1ng = byt[rb.beginvale + 20] * 256 + byt[rb.beginvale + 21];
                rb.barcode2ng = byt[rb.beginvale + 22] * 256 + byt[rb.beginvale + 23];
                rb.barcode3ng = byt[rb.beginvale + 24] * 256 + byt[rb.beginvale + 25];
                rb.barcode4ng = byt[rb.beginvale + 26] * 256 + byt[rb.beginvale + 27];
                Array.Copy(byt, rb.beginvale + 1000, BatteryCode1Tmp, 0, cellLength);
                Array.Copy(byt, rb.beginvale + 1030, BatteryCode2Tmp, 0, cellLength);
                Array.Copy(byt, rb.beginvale + 1060, BatteryCode3Tmp, 0, cellLength);
                Array.Copy(byt, rb.beginvale + 1090, BatteryCode4Tmp, 0, cellLength);


                rb.barcode1 = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode1Tmp, cellLength)));
                rb.barcode2 = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode2Tmp, cellLength)));
                rb.barcode3 = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode3Tmp, cellLength)));
                rb.barcode4 = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode4Tmp, cellLength)));

                if (rb.checkbarcodereading > 0 || rb.inplacebarcodereading > 0)
                {
                    //业务逻辑处理
                    ShowEquipmentTaskInfo(rb);
                }
            }
        }

        private void BindPutRoboInfo(string robotnum, byte[] byt) //组盘
        {
            RobotInfo rb = RobotsInfo.Find(
                delegate (RobotInfo ss)
                {
                    return ss.robotnum == robotnum;
                }
            );
            if (robotnum == "9004")
            { }
            if (rb != null)
            {
                int cellLength = 30;

                rb.wcstaskno = (byt[rb.beginvale + 0] * 256 + byt[rb.beginvale + 1]).ToString();//WCS任务号
                rb.rbtaskno = byt[rb.beginvale + 2] * 256 + byt[rb.beginvale + 3];//机器人任务号 
                rb.rbstate = byt[rb.beginvale + 4] * 256 + byt[rb.beginvale + 5];//机器人运行状态  0，未启动；1，自动；2，手动；3，故障
                rb.rbnumber = byt[rb.beginvale + 6] * 256 + byt[rb.beginvale + 7];//当前装盘数量
                rb.pickbarcodereading = byt[rb.beginvale + 8] * 256 + byt[rb.beginvale + 9];//机器人装盘条码读取信号 0，无指令；1，读取条码(RB)；2,读取完成(WCS);3,读到2后置0(RB)
                rb.alarmCode = byt[rb.beginvale + 10] * 256 + byt[rb.beginvale + 11];//故障代码
                                                                                     // rb.alarmCode = byt[rb.beginvale + 12] * 256 + byt[rb.beginvale + 13];//故障代码
                robotStatus[rb.name] = rb.rbstate.ToString();
                //获取rb.cellNum个电芯条码，放入BarCodeList
                rb.BarCodeList.Clear();
                for (int i = 0; i < rb.cellNum; i++)
                {
                    byte[] BatteryCodeTmp = new byte[30];
                    string barCode = string.Empty;
                    Array.Copy(byt, rb.beginvale + 1000 + 30 * i, BatteryCodeTmp, 0, 30);
                    barCode = clearbarcodeFormat(Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCodeTmp, cellLength)).Replace(" ", "").Trim('\0').Trim('\r').Trim('\n'));
                    // barCode = Encoding.ASCII.GetString(BatteryCodeTmp).Replace(" ", "").Trim('\0').Trim('\r').Trim('\n');
                    // barCode = highlowbatteryInfo(barCode, cellLength);
                    rb.BarCodeList.Add(barCode);
                }
                rb.positions.Clear();
                rb.ResultList.Clear();
                for (int i = 0; i < rb.cellNum; i++)
                {
                    string position = (byt[rb.beginvale + 20 + 2 * i] * 256 + byt[rb.beginvale + 20 + 2 * i + 1]).ToString().Trim('\r').Trim('\n');
                    rb.positions.Add(position);
                    rb.ResultList.Add(position);
                }
                //业务逻辑处理
                ShowEquipmentTaskInfo(rb);
            }
        }

        private void BindPickRoboInfo(string robotnum, byte[] byt)
        {
            RobotInfo rb = RobotsInfo.Find(
                 delegate (RobotInfo ss)
                 {
                     return ss.robotnum == robotnum;
                 }
             );
            if (rb != null)
            {
                rb.wcstaskno = (byt[rb.beginvale + 0] * 256 + byt[rb.beginvale + 1]).ToString();//机械手状态 0， 未启动；1，自动；2，手动；3，故障
                rb.rbtaskno = byt[rb.beginvale + 2] * 256 + byt[rb.beginvale + 3]; //机器人任务号 0，无任务；1，拆盘中
                rb.rbstate = byt[rb.beginvale + 4] * 256 + byt[rb.beginvale + 5];
                rb.rbpickcount = byt[rb.beginvale + 6] * 256 + byt[rb.beginvale + 7];//拆完数量
                rb.trayreading = byt[rb.beginvale + 8] * 256 + byt[rb.beginvale + 9];//0，无指令；1，申请下发指令(RB)；2,完成下发(WCS)；3，分拣完成(RB)；WCS读3后置4(WCS)
                                                                                     // rb.heart = byt[10] * 256 + byt[11];
                rb.alarmCode = byt[rb.beginvale + 10] * 256 + byt[rb.beginvale + 11];//故障代码
                robotStatus[rb.name] = rb.rbstate.ToString();
                rb.BarCodeList.Clear();
                rb.ResultList.Clear();
                for (int i = 0; i < rb.cellNum; i++)
                {
                    byte[] ResultTmp = new byte[2];
                    string result = string.Empty;
                    result = (byt[rb.beginvale + 20 + (2 * i)] * 256 + byt[rb.beginvale + 20 + (2 * i + 1)]).ToString().Trim('\r').Trim('\n'); ;
                    rb.ResultList.Add(result);

                    byte[] BatteryCodeTmp = new byte[30];
                    string barCode = string.Empty;
                    Array.Copy(byt, rb.beginvale + 1000 + 30 * i, BatteryCodeTmp, 0, 30);
                    barCode = Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCodeTmp, BatteryCodeTmp.Length)).Replace(" ", "").Trim('\0').Trim('\r').Trim('\n');
                    rb.BarCodeList.Add(barCode);
                }
                //业务逻辑处理
                ShowEquipmentTaskInfo(rb);
            }
        }
        /// <summary>
        /// 码垛机
        /// </summary>
        /// <param name="robotnum"></param>
        /// <param name="byt"></param>
        private void StackingRoboInfo(string robotnum, byte[] byt)
        {
            RobotInfo rb = RobotsInfo.Find(
                 delegate (RobotInfo ss)
                 {
                     return ss.robotnum == robotnum;
                 }
             );
            if (rb != null)
            {
                int cellLength = 14;
                byte[] BatteryCode1Tmp = new byte[cellLength];
                byte[] BatteryCode1Tmp1 = new byte[cellLength];


                /*rb.wcstaskno = (byt[rb.beginvale + 0] * 256 + byt[rb.beginvale + 1]).ToString();//机械手状态 0， 未启动；1，自动；2，手动；3，故障
                rb.rbtaskno = byt[rb.beginvale + 2] * 256 + byt[rb.beginvale + 3]; //机器人任务号 0，无任务；1，拆盘中
                rb.rbstate = byt[rb.beginvale + 4] * 256 + byt[rb.beginvale + 5];
                rb.rbpickcount = byt[rb.beginvale + 6] * 256 + byt[rb.beginvale + 7];//码垛数量
                rb.checkbarcodereading = byt[rb.beginvale + 8] * 256 + byt[rb.beginvale + 9];//码垛交互指令 0，无指令；1，申请下发指令(RB)；2,完成下发(WCS)；3，分拣完成(RB)；WCS读3后置4(WCS)
                                                                                             // rb.heart = byt[10] * 256 + byt[11];
                rb.alarmCode = byt[rb.beginvale + 10] * 256 + byt[rb.beginvale + 11];//故障代码
                rb.inplacebarcodereading = byt[rb.beginvale + 12] * 256 + byt[rb.beginvale + 13];//单盘扫码交互指令
                robotStatus[rb.name] = rb.rbstate.ToString();
                rb.barcode1ng = byt[rb.beginvale + 20] * 256 + byt[rb.beginvale + 21];
                Array.Copy(byt, rb.beginvale + 1000, BatteryCode1Tmp, 0, cellLength);
                rb.barcode1 = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode1Tmp, cellLength)));*/
                rb.checkbarcodereading = byt[rb.beginvale + 0] * 256 + byt[rb.beginvale + 1];//码垛交互指令 0，无指令；1，申请下发指令(RB)；2,完成下发(WCS)；3，分拣完成(RB)；WCS读3后置4(WCS) 
                rb.inplacebarcodereading = byt[rb.beginvale + 2] * 256 + byt[rb.beginvale + 3];//单盘扫码交互指令
                rb.rbstate = byt[rb.beginvale + 4] * 256 + byt[rb.beginvale + 5];
                rb.alarmCode = byt[rb.beginvale + 10] * 256 + byt[rb.beginvale + 11];//故障代码
                robotStatus[rb.name] = rb.rbstate.ToString();
                rb.barcode1ng = byt[rb.beginvale + 20] * 256 + byt[rb.beginvale + 21];
                Array.Copy(byt, rb.beginvale + 1000, BatteryCode1Tmp, 0, cellLength);
                rb.barcode1 = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode1Tmp, cellLength)));//扫码条码
                Array.Copy(byt, rb.beginvale + 3000, BatteryCode1Tmp1, 0, cellLength);
                rb.traycode = clearbarcodeInfo(System.Text.Encoding.ASCII.GetString(highlowbatteryInfo(BatteryCode1Tmp1, cellLength)));//码垛条码

                if (rb.checkbarcodereading ==1 || rb.inplacebarcodereading==1)
                //业务逻辑处理
                ShowEquipmentTaskInfo(rb);
            }
        }

        /// <summary>
        /// 重新连接
        /// </summary>
        /// <param name="strIP"></param>
        /// <param name="retryCount"></param>
        private void ReConnect(string strIP, int retryCount = 1)
        {
            if (RobotReConnectPLC != null)
            {
                if (PLCList.ContainsKey(strIP))
                {
                    bool flag = true;
                    int i_Count = 1;
                    while (flag)
                    {
                        if (i_Count > retryCount)
                        {
                            break;
                        };
                        PLCList[strIP].ConnectClose();
                        ShowMsg("Break", string.Format("PLC：{0}，尝试连接次数：{1}", strIP, i_Count));
                        OperateResult connect = PLCList[strIP].ConnectServer();
                        if (connect.IsSuccess)
                        {
                            ShowMsg("Break", string.Format("PLC：{0}，连接成功", strIP, i_Count));
                            flag = false;
                            break;
                        }
                        else
                        {
                            i_Count++;
                        }
                    }
                }
            }
        }

        public bool WriteRobotInfo(string robottype, string rbnum, RobotCommand cmd, string[] str, int index, int length, int cellNum)
        {
            if (robottype == "PutRobot")
            {
                switch (cmd)
                {
                    case RobotCommand.InplaceResult:
                        int inplace = 8;
                        return Write(rbnum, "2", inplace, 1);//到位申请                       
                    case RobotCommand.CheckResult:
                        byte[] byt = new byte[(str.Length - 1) * 2];
                        for (int i = 1; i < str.Length; i++)
                        {
                            byte[] bytemp = ConvertHelper.GetHexBytes(str[i]);
                            Array.Copy(bytemp, 0, byt, (i - 1) * 2, 2);
                        }
                        if (cellNum == 2)
                        {
                            byte[] bytemp = new byte[byt.Length];
                            Array.Copy(byt, index == 12 ? 0 : 4, bytemp, 0, 4);
                            if (!WriteBytes(rbnum, bytemp, index, 4))
                            {
                                return false;
                            }
                            //if (index == 12)
                            //{
                            //    Write(rbnum, str[1], 12, 2);//写入1号电池校验结果
                            //    Write(rbnum, str[2], 14, 2);//写入2号电池校验结果
                            //}
                            //else if (index == 16)
                            //{
                            //    Write(rbnum, str[3], 16, 2);//写入3号电池校验结果
                            //    Write(rbnum, str[4], 18, 2);//写入4号电池校验结果
                            //}
                        }
                        else
                        {
                            //Write(rbnum, str[1], 12, 2);//写入1号电池校验结果
                            //Write(rbnum, str[2], 14, 2);//写入2号电池校验结果
                            //Write(rbnum, str[3], 16, 2);//写入3号电池校验结果
                            //Write(rbnum, str[4], 18, 2);//写入4号电池校验结果

                            if (!WriteBytes(rbnum, byt, index, cellNum * 2))
                            {
                                return false;
                            }
                        }
                        //  return Write(rbnum, str[0], 8, 2);//写入电池校验结果str[0
                        return Write(rbnum, str[0], 8, 1);//写入电池校验结果str[0]

                    case RobotCommand.BindResult:
                        return Write(rbnum, str[0], index, length);//抓取完成申请
                                                                   //byte[] byt1 = new byte[cellNum];
                                                                   //for (int i=0;i<str.Length;i++)
                                                                   //{
                                                                   //    byte[] bytemp = ConvertHelper.GetHexBytes(str[i]);
                                                                   //    Array.Copy(bytemp,0,byt1,i*2,2);
                                                                   //}
                                                                   //return Write(rbnum, byt1, index, length);//抓取完成申请

                    case RobotCommand.completePut:
                        return Write(rbnum, str[0], 8, 1);//整盘完成申请
                }

            }
            else if (robottype == "PickRobot")
            {
                switch (cmd)
                {

                    case RobotCommand.PickCommand:
                        byte[] byt = new byte[cellNum * 2];
                        byte[] byt2 = new byte[30 * cellNum];
                        byte[] BatteryCodeTmp = new byte[30];

                        for (int i = 0; i < str.Length; i++)
                        {
                            if (i < cellNum)
                            {
                                byte[] bytemp = ConvertHelper.GetHexBytes(str[i]);
                                Array.Copy(bytemp, 0, byt, i * 2, 2);
                            }
                            else
                            {
                                //byte[] byt3 = new byte[30];
                                //BatteryCodeTmp = System.Text.ASCIIEncoding.ASCII.GetBytes(str[i].PadRight(30, '\0'));
                                //byt3 = highlowbatteryInfo(BatteryCodeTmp, 30);
                                //if (!WriteBytes(rbnum, byt3, 1000+ 30 * (i - cellNum),  30))                                  
                                //   return false;


                                //BatteryCodeTmp = System.Text.ASCIIEncoding.ASCII.GetBytes(str[i].PadRight(30, '\0'));
                                //Array.Copy(highlowbatteryInfo(BatteryCodeTmp,30), 0, byt2, 30 * (i - cellNum), 30);

                                if (i % 4 == 0) //每4个电芯更新一次
                                {
                                    byte[] byt3 = new byte[120];
                                    for (int n = 0; n < 4; n++)
                                    {
                                        BatteryCodeTmp = System.Text.ASCIIEncoding.ASCII.GetBytes(str[i + n].PadRight(30, '\0'));
                                        Array.Copy(highlowbatteryInfo(BatteryCodeTmp, 30), 0, byt3, 30 * n, 30);
                                    }
                                    if (!WriteBytes(rbnum, byt3, 1000 + 30 * (i - cellNum), 120))
                                        return false;
                                }
                            }
                        }
                        if (WriteBytes(rbnum, byt, 20, cellNum * 2))//写入电芯状态
                        {

                            //  if (WriteBytes(rbnum, byt2, 1000, cellNum * 30))
                            //  {
                            if (Write(rbnum, "2", 8, 1))//回复申请
                                return true;
                            else
                                return false;
                            //  }
                        }
                        break;

                    //写入电芯状态
                    case RobotCommand.ReWorkResponse:
                        if (Write(rbnum, "2", 8, 1))//回复装盘数量
                        {
                            return true;
                        }
                        else
                        {
                            return false;
                        }
                    case RobotCommand.enableReworkPut:
                        if (WriteBit(rbnum, "2", 14, 1))//允许本地组复测盘
                        {
                            return true;
                        }
                        else
                        {
                            return false;
                        }
                    case RobotCommand.completePick:
                        if (Write(rbnum, "4", 8, 1))//拆盘完成回复信息
                        {
                            return true;
                        }
                        else
                        {
                            return false;
                        }
                    case RobotCommand.SendReworkBarcode://发送复测工位上的托盘条码给机械手
                        byte[] byt1 = new byte[2];
                        Array.Copy(ConvertHelper.GetHexBytes(str[0]), 0, byt1, 0, 2);
                        if (WriteBytes(rbnum, byt1, 16, 2))//写入电芯状态
                        {
                            if (WriteBit(rbnum, "2", 19, 1))//回复申请
                                return true;
                            else
                                return false;
                        }
                        else
                        {
                            return false;
                        }
                    case RobotCommand.completePut:
                        return WriteBit(rbnum, "7", 19, 1);//抓取完成申请
                    case RobotCommand.PickCommand_hc:
                        if (WriteBit(rbnum, "2", 4, 1))//抓取完成申请
                            return WriteBit(rbnum, str[0], 5, 1);
                        break;
                }
            }
            else if (robottype == "StackingRobot")
            {
                switch (cmd)
                {
                    case RobotCommand.ScanTrayCommand:
                        int inplace = 2;
                        return Write(rbnum, str[0], inplace, 1);//到位申请                       
                    case RobotCommand.TrayGradeCommand:
                        byte[] byt = new byte[(str.Length - 1) * 2];
                        for (int i = 1; i < str.Length; i++)
                        {
                            byte[] bytemp = ConvertHelper.GetHexBytes(str[i]);
                            Array.Copy(bytemp, 0, byt, (i - 1) * 2, 2);
                        }
                        if (cellNum == 2)
                        {
                            byte[] bytemp = new byte[byt.Length];
                            Array.Copy(byt, index == 12 ? 0 : 4, bytemp, 0, 4);
                            if (!WriteBytes(rbnum, bytemp, index, 4))
                            {
                                return false;
                            }
                        }
                        else
                        {
                            if (!WriteBytes(rbnum, byt, index, cellNum * 2))
                            {
                                return false;
                            }
                        }
                        return Write(rbnum, str[0], 0, 1);//写入电池校验结果str[0]
                }
            }
            return false;
        }
        public bool ReadRobotInfo(RobotInfo rb, int index, int length, ref Byte[] rbBuffer)
        {
            if (ReadPlc(PLCList[rb.key_ip], rb.DBAddress, index, length, ref rbBuffer))
            {

                return true;
            }
            return false;
        }
        private bool Write(string rbnum, string value, int index, int length = 0)
        {
            RobotInfo rb = RobotsInfo.Find(
                 delegate (RobotInfo item)
                 {
                     return item.robotnum == rbnum;
                 }
             );
            if (rb != null)
            {
                byte[] byt;
                if (length == 0)
                    byt = new byte[1];
                else
                    byt = new byte[length * 2];
                Array.Copy(ConvertHelper.GetHexBytes(value), 0, byt, 0, 2);
                if (true)//调试模式
                {
                    /* if (WritePlc(PLCList[rb.key_ip], rb.DBAddress, rb.start + index, byt))
                     {
                         return true;
                     }*/
                    if (modbusWriteClients[rb.key_ip].Client.Connected)
                    {
                        //if (writeService.WriteByte(rb.start, byt, modbusWriteClients[rb.key_ip], rbnum).IsSucceed)
                        if (writeService.WriteByte((rb.beginvale + index) / 2, byt, modbusWriteClients[rb.key_ip], rbnum).IsSucceed)
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }
        private bool WriteBit(string rbnum, string value, int index = 0, int length = 1)
        {

            RobotInfo rb = RobotsInfo.Find(
                 delegate (RobotInfo item)
                 {
                     return item.robotnum == rbnum;
                 }
             );
            if (rb != null)
            {
                byte[] byt = new byte[length];
                byte byttem = Convert.ToByte(value);
                byte[] bytArray = new byte[2];
                bytArray[0] = byttem;
                Array.Copy(bytArray, 0, byt, 0, 1);

                /* if (WritePlc(PLCList[rb.key_ip], rb.DBAddress, rb.start + index, byt))
                     return true;
                 else
                     return false;*/
                if (modbusWriteClients[rb.key_ip].Client.Connected)
                {
                    if (writeService.WriteByte(rb.start + index / 2, byt, modbusWriteClients[rb.key_ip], "W" + rb.belong).IsSucceed)
                    {
                        return true;
                    }
                    else
                        return false;
                }
            }
            return false;
        }
        private bool WriteBytes(string rbnum, byte[] btys, int index = 0, int length = 1)
        {

            RobotInfo rb = RobotsInfo.Find(
                 delegate (RobotInfo item)
                 {
                     return item.robotnum == rbnum;
                 }
             );
            if (rb != null)
            {
                /*if (WritePlc(PLCList[rb.key_ip], rb.DBAddress, rb.start + index, btys))
                    return true;
                else
                    return false;*/
                if (modbusWriteClients[rb.key_ip].Client.Connected)
                {
                    //if (writeService.WriteByte(rb.start + index / 2, btys, modbusWriteClients[rb.key_ip], "W" + rb.belong).IsSucceed)
                    if (writeService.WriteByte((rb.beginvale + index) / 2, btys, modbusWriteClients[rb.key_ip], "W" + rb.belong).IsSucceed)
                    {
                        return true;
                    }
                    else
                        return false;
                }
            }
            return false;
        }

        public string getRobotStatus(string name)
        {
            if (robotStatus.ContainsKey(name))
                return robotStatus[name];
            else
                return "3";
        }
    }
}
